#pragma once 
#include "Particle.h"
#include "Box.h"
#include <GL/gl.h>
class ParticleConstraint
{
private:
	float rest_distance; // the length between particle p1 and p2 in rest configuration

public:
	Object *p1, *p2; // the two particles that are connected through this constraint

	ParticleConstraint(Object *p1, Object *p2) :  p1(p1),p2(p2)
	{
		

		//std::cout <<"p2";
		/*for(int i =0; i < 3 ; i ++)
		{
			std::cout << p2->getPosition()[i] <<" ";

		}*/

		char *in;
	//	std::cin >> in;

		//Vec3 *v1 =(Vec3 *)p1->getPosition();
		//Vec3 *v2 = (Vec3 *)p2->getPosition();	
		//Vec3 vec = (*v1)-(*v2); //not sure if casting works and the minus sign returns proper vec.
		
		Vec3 v1((float)p1->getPosition()[0], (float)p1->getPosition()[1],(float)p1->getPosition()[2]);
		Vec3 v2((float)p2->getPosition()[0], (float)p2->getPosition()[1],(float)p2->getPosition()[2]);
		rest_distance = (v1-v2).length();
	//	std::cout << rest_distance;
	//	std::cin >> in;
	}

	


	/* This is one of the important methods, where a single constraint between two particles p1 and p2 is solved
	the method is called by Cloth.time_step() many times per frame*/
	void satisfyConstraint()
	{


		Vec3 v1((float)p1->getPosition()[0], (float)p1->getPosition()[1],(float)p1->getPosition()[2]);
		Vec3 v2((float)p2->getPosition()[0], (float)p2->getPosition()[1],(float)p2->getPosition()[2]);
		// this is the key, can not be v1 - v2, has to be v2 - v1
		Vec3 p1_to_p2 = (v2-v1);
		float current_distance = p1_to_p2.length();
		//std::cout << "current_distance" <<current_distance <<"\n";

		Vec3 correctionVector = p1_to_p2*(1 - rest_distance/current_distance); // The offset vector that could moves p1 into a distance of rest_distance to p2
		Vec3 correctionVectorHalf = correctionVector*0.5; // Lets make it half that length, so that we can move BOTH p1 and p2.
		//dReal *pos = (dReal*) &((*v1+correctionVectorHalf));
		//p1->setPosition(pos[0],pos[1],pos[2]);

		//offset 
		float *pos1 = (v1+correctionVectorHalf).f;		
		float *pos2 = (v2-correctionVectorHalf).f;

		if( dBodyIsEnabled(p1->body)==1)
		p1->setPosition(pos1[0],pos1[1],pos1[2]);
		if( dBodyIsEnabled(p2->body)==1)
		p2->setPosition(pos2[0],pos2[1],pos2[2]);
		//p1->setPosition(v1[0]+correctionVectorHalf[0],v1[1]+correctionVectorHalf[1],v1[2]+correctionVectorHalf[2]);

		/*
		Vec3 p1_to_p2 = p2->getPos()-p1->getPos(); // vector from p1 to p2
		float current_distance = p1_to_p2.length(); // current distance between p1 and p2
		Vec3 correctionVector = p1_to_p2*(1 - rest_distance/current_distance); // The offset vector that could moves p1 into a distance of rest_distance to p2
		Vec3 correctionVectorHalf = correctionVector*0.5; // Lets make it half that length, so that we can move BOTH p1 and p2.
		p1->offsetPos(correctionVectorHalf); // correctionVectorHalf is pointing from p1 to p2, so the length should move p1 half the length needed to satisfy the constraint.
		p2->offsetPos(-correctionVectorHalf); // we must move p2 the negative direction of correctionVectorHalf since it points from p2 to p1, and not p1 to p2.	
	*/
		char in[256];
		//std::cin >>  in;
		//std::cin.getline(in,256);
	}

};
